

#define BOOST_TEST_MODULE MMM_MODELPROCESSOR_TEST

#include <MMM/Model/Model.h>
#include <MMM/Motion/Legacy/LegacyMotion.h>
#include <MMM/Model/ModelProcessorWinter.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <string>

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

BOOST_AUTO_TEST_CASE(testModelProcessorWinter)
{
    const std::string modelString =
		"<?xml version='1.0' encoding='UTF-8'?>"
		"<Robot Type='DemoModel' RootNode='Joint1'>"
		"	<RobotNode name='Joint1'>"
		"		<Visualization enable='true'>"
		"			<File type='Inventor'>test.iv</File>"
		"			<CoordinateAxis type='Inventor' enable='true' scaling='1' text='Axis1'/>"
		"		</Visualization>"
		"		<CollisionModel>"
		"			<File type='Inventor'>test.iv</File>"
		"		</CollisionModel>"
		"		<Child name='Joint2'/>"
		"	</RobotNode>"
		"	<RobotNode name='Joint2'>"
		"		<Transform>"
		"			<Translation x='100' y='0' z='0' units='mm'/>"
		"		</Transform>"
		"		<Joint type='revolute' offset='0'>"
		"			<Limits unit='degree' lo='-90' hi='45'/>"
		"			<Axis x='0' y='0' z='1'/>"
		"		</Joint>"
		"		<Visualization enable='true'>"
		"			<File type='Inventor'>test.iv</File>"
		"		</Visualization>"
		"		<CollisionModel>"
		"			<File boundingbox='true' type='Inventor'>test.iv</File>"
		"		</CollisionModel>"
		"	</RobotNode>"
		"</Robot>";
	MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
	MMM::ModelPtr m = r->createModelFromString(modelString);
	BOOST_REQUIRE(m);
	BOOST_REQUIRE(m->getRoot()=="Joint1");
	BOOST_REQUIRE(m->getName()=="DemoModel");
	BOOST_REQUIRE(m->getModels().size()==2);
	MMM::ModelNodePtr mod1 = m->getModels().at(0);
	BOOST_REQUIRE(mod1);
	BOOST_REQUIRE(mod1->name=="Joint1");
	BOOST_REQUIRE(mod1->joint.jointType==MMM::eFixed);
	MMM::ModelNodePtr mod2 = m->getModels().at(1);
	BOOST_REQUIRE(mod2);
	BOOST_REQUIRE(mod2->name=="Joint2");
	BOOST_REQUIRE(mod2->joint.jointType==MMM::eRevolute);
    BOOST_REQUIRE_CLOSE(mod2->localTransformation.block(0,3,3,1).norm(), 0.1f, 0.01f);

    MMM::ModelProcessorFactoryPtr modelFactory = MMM::ModelProcessorFactory::fromName("Winter", NULL);
    BOOST_REQUIRE(modelFactory);

    MMM::ModelProcessorPtr modelProcessor = modelFactory->createModelProcessor();
    BOOST_REQUIRE(modelProcessor);
    MMM::ModelProcessorWinterPtr mpWinter = boost::dynamic_pointer_cast<MMM::ModelProcessorWinter>(modelProcessor);
    BOOST_REQUIRE(mpWinter);
    bool setupOK = mpWinter->setup(2.0f, 50.0f);
    BOOST_REQUIRE(setupOK);

    setupOK = mpWinter->setupSegmentLength("Joint2", 0.3f);
    BOOST_REQUIRE(setupOK);




    MMM::ModelPtr mmmModel2 = modelProcessor->convertModel(m);
    BOOST_REQUIRE(mmmModel2);

    float height1 = m->getHeight();
    BOOST_REQUIRE_CLOSE(height1, 1.0f, 0.001f);
    float height2 = mmmModel2->getHeight();
    BOOST_REQUIRE_CLOSE(height2, 2.0f, 0.001f);

    MMM::ModelNodePtr n1 = m->getModelNode("Joint2");
    BOOST_REQUIRE(n1);
    float l1 = n1->localTransformation.block(0, 3, 3, 1).norm();
    BOOST_REQUIRE_CLOSE(l1, 0.1f, 0.001f);

    MMM::ModelNodePtr n2 = mmmModel2->getModelNode("Joint2");
    BOOST_REQUIRE(n2);
    float l2 = n2->localTransformation.block(0, 3, 3, 1).norm();
    BOOST_REQUIRE_CLOSE(l2, 0.3f, 0.001f);


}


// BOOST_AUTO_TEST_SUITE_END()
